#include "RigidCoordCalibration.h"

#include <opencv2/core.hpp>

#include <memath/meTransform.h>

using namespace std;
using namespace cv;

/// 给定两点, 生成矩阵A中的方程p1, p2, p3
static void fillMatrixAA(Matrix4d &A, double *p1, double *p2, double *p3, Eigen::Vector3d &pt1, Eigen::Vector3d &pt2)
{
    p1[0] = A(0,0)*pt1[0] - pt2[0];
    p1[1] = A(0,0)*pt1[1] - pt2[1];
    p1[2] = A(0,0)*pt1[2] - pt2[2];
    p1[3] = A(0,0) -1;

    p1[4] = A(0,1)*pt1[0];
    p1[5] = A(0,1)*pt1[1];
    p1[6] = A(0,1)*pt1[2];
    p1[7] = A(0,1);

    p1[8]  = A(0,2)*pt1[0];
    p1[9]  = A(0,2)*pt1[1];
    p1[10] = A(0,2)*pt1[2];
    p1[11] = A(0,2);

    p2[0] = A(1,0)*pt1[0];
    p2[1] = A(1,0)*pt1[1];
    p2[2] = A(1,0)*pt1[2];
    p2[3] = A(1,0);

    p2[4] = A(1,1)*pt1[0] - pt2[0];
    p2[5] = A(1,1)*pt1[1] - pt2[1];
    p2[6] = A(1,1)*pt1[2] - pt2[2];
    p2[7] = A(1,1) - 1;

    p2[8]  = A(1,2)*pt1[0];
    p2[9]  = A(1,2)*pt1[1];
    p2[10] = A(1,2)*pt1[2];
    p2[11] = A(1,2);

    p3[0] = A(2,0)*pt1[0];
    p3[1] = A(2,0)*pt1[1];
    p3[2] = A(2,0)*pt1[2];
    p3[3] = A(2,0);

    p3[4] = A(2,1)*pt1[0];
    p3[5] = A(2,1)*pt1[1];
    p3[6] = A(2,1)*pt1[2];
    p3[7] = A(2,1);

    p3[8]  = A(2,2)*pt1[0] - pt2[0];
    p3[9]  = A(2,2)*pt1[1] - pt2[1];
    p3[10] = A(2,2)*pt1[2] - pt2[2];
    p3[11] = A(2,2) - 1;
}

/// 通过B空间的两组点获取A空间的点
void RigidCoordCalibration::calRigidMatrix_AfrB(Matrix4d &m_A1frA2, Matrix4d &m_A1frA3,
                     vector<Eigen::Vector3d> &pts_B1, vector<Eigen::Vector3d> &pts_B2, vector<Eigen::Vector3d> &pts_B3,
                     Matrix4d &m_AfrB)
{
    Matrix4d &A1 = m_A1frA2;
    Matrix4d &A2 = m_A1frA3;

    uint nPoint = pts_B1.size();
    Mat AA = Mat(nPoint*6,12,CV_64F);
    Mat BB = Mat(nPoint*6,1,CV_64F);
    for(uint i=0; i<nPoint; i++) {
        Eigen::Vector3d &pt1 = pts_B1[i];
        Eigen::Vector3d &pt2 = pts_B2[i];
        Eigen::Vector3d &pt3 = pts_B3[i];


        fillMatrixAA(A1, AA.ptr<double>(i*6), AA.ptr<double>(i*6+1), AA.ptr<double>(i*6+2), pt1, pt2);
        fillMatrixAA(A2, AA.ptr<double>(i*6+3), AA.ptr<double>(i*6+4), AA.ptr<double>(i*6+5), pt1, pt3);
        BB.at<double>(i*6,0) = -A1(0,3);
        BB.at<double>(i*6+1,0) = -A1(1,3);
        BB.at<double>(i*6+2,0) = -A1(2,3);
        BB.at<double>(i*6+3,0) = -A2(0,3);
        BB.at<double>(i*6+4,0) = -A2(1,3);
        BB.at<double>(i*6+5,0) = -A2(2,3);
    }
    // cout << "AA: " << AA << endl;

    Mat B = Mat(12,1,CV_64F);
    solve(AA, BB, B, DECOMP_SVD);
    //SVD::solveZ(AA, B);
    // cout << "B: \n" << B << endl;

    double *pB = B.ptr<double>();
    m_AfrB(0,0) = pB[0]; m_AfrB(0,1) = pB[1]; m_AfrB(0,2) = pB[2]; m_AfrB(0,3) = pB[3];
    m_AfrB(1,0) = pB[4]; m_AfrB(1,1) = pB[5]; m_AfrB(1,2) = pB[6]; m_AfrB(1,3) = pB[7];
    m_AfrB(2,0) = pB[8]; m_AfrB(2,1) = pB[9]; m_AfrB(2,2) = pB[10]; m_AfrB(2,3) = pB[11];
    m_AfrB(3,0) =0     ; m_AfrB(3,1) = 0    ; m_AfrB(3,2) = 0     ; m_AfrB(3,3) = 1;
    //cout << "m_AfrB: " << m_AfrB << endl;
}

void RigidCoordCalibration::calRigidMatrix_AfrB(Matrix4d &m_A1frA2, Matrix4d &m_A1frA3, Matrix4d &m_A1frA4, std::vector<Eigen::Vector3d> &pts_B1, std::vector<Eigen::Vector3d> &pts_B2, std::vector<Eigen::Vector3d> &pts_B3, std::vector<Eigen::Vector3d> &pts_B4, Matrix4d &m_AfrB)
{
    Matrix4d &A1 = m_A1frA2;
    Matrix4d &A2 = m_A1frA3;
    Matrix4d &A3 = m_A1frA4;

    int factor = 9;
    uint nPoint = pts_B1.size();
    Mat AA = Mat(nPoint*factor,12,CV_64F);
    Mat BB = Mat(nPoint*factor,1,CV_64F);
    for(uint i=0; i<nPoint; i++) {
        Eigen::Vector3d &pt1 = pts_B1[i];
        Eigen::Vector3d &pt2 = pts_B2[i];
        Eigen::Vector3d &pt3 = pts_B3[i];
        Eigen::Vector3d &pt4 = pts_B4[i];


        fillMatrixAA(A1, AA.ptr<double>(i*factor), AA.ptr<double>(i*factor+1), AA.ptr<double>(i*factor+2), pt1, pt2);
        fillMatrixAA(A2, AA.ptr<double>(i*factor+3), AA.ptr<double>(i*factor+4), AA.ptr<double>(i*factor+5), pt1, pt3);
        fillMatrixAA(A3, AA.ptr<double>(i*factor+6), AA.ptr<double>(i*factor+7), AA.ptr<double>(i*factor+8), pt1, pt4);
        BB.at<double>(i*factor,0) = -A1(0,3);
        BB.at<double>(i*factor+1,0) = -A1(1,3);
        BB.at<double>(i*factor+2,0) = -A1(2,3);
        BB.at<double>(i*factor+3,0) = -A2(0,3);
        BB.at<double>(i*factor+4,0) = -A2(1,3);
        BB.at<double>(i*factor+5,0) = -A2(2,3);
        BB.at<double>(i*factor+6,0) = -A3(0,3);
        BB.at<double>(i*factor+7,0) = -A3(1,3);
        BB.at<double>(i*factor+8,0) = -A3(2,3);
    }
    //cout << "AA: " << AA << endl;

    Mat B = Mat(12,1,CV_64F);
    solve(AA, BB, B, DECOMP_SVD);
    //SVD::solveZ(AA, B);
    //cout << "B: \n" << B << endl;

    double *pB = B.ptr<double>();
    m_AfrB(0,0) = pB[0]; m_AfrB(0,1) = pB[1]; m_AfrB(0,2) = pB[2]; m_AfrB(0,3) = pB[3];
    m_AfrB(1,0) = pB[4]; m_AfrB(1,1) = pB[5]; m_AfrB(1,2) = pB[6]; m_AfrB(1,3) = pB[7];
    m_AfrB(2,0) = pB[8]; m_AfrB(2,1) = pB[9]; m_AfrB(2,2) = pB[10]; m_AfrB(2,3) = pB[11];
    m_AfrB(3,0) =0     ; m_AfrB(3,1) = 0    ; m_AfrB(3,2) = 0     ; m_AfrB(3,3) = 1;
    //cout << "m_AfrB: " << m_AfrB << endl;
}

/// 通过B空间的两组点获取A空间的点
void RigidCoordCalibration::calRigidMatrix_AfrB(vector<Matrix4d> &m_AxfrAy,
                     vector<vector<Eigen::Vector3d>> &pts_By,
                     Matrix4d &m_AfrB)
{
    vector<Matrix4d> &Ay = m_AxfrAy;
    vector<vector<Eigen::Vector3d>> &pts = pts_By;
    if (pts.size() < 2) {
        return;
    }

    uint nSize = pts.size()-1;
    uint nPoint = pts[0].size();
    Mat AA = Mat(nPoint*nSize*3,12,CV_64F);
    Mat BB = Mat(nPoint*nSize*3,1,CV_64F);
    Matrix4d A0inv = Ay[0].getInverseHomogeneous();
    vector<Eigen::Vector3d> &pts_B1 = pts[0];
    for(uint j=0; j < nSize; j++) {
        Matrix4d A1 = A0inv*Ay[j+1];
        vector<Eigen::Vector3d> &pts_B2 = pts[j+1];

        int offset = j*nPoint*3;
        for(uint i=0; i<nPoint; i++) {
            Eigen::Vector3d &pt1 = pts_B1[i];
            Eigen::Vector3d &pt2 = pts_B2[i];

            fillMatrixAA(A1, AA.ptr<double>(offset+i*3+0), AA.ptr<double>(offset+i*3+1), AA.ptr<double>(offset+i*3+2), pt1, pt2);
            BB.at<double>(offset+i*3,0) = -A1(0,3);
            BB.at<double>(offset+i*3+1,0) = -A1(1,3);
            BB.at<double>(offset+i*3+2,0) = -A1(2,3);
        }
    }

    cout << "AA: " << AA << endl;

    Mat B = Mat(12,1,CV_64F);
    solve(AA, BB, B, DECOMP_SVD);
    //SVD::solveZ(AA, B);
    // cout << "B: \n" << B << endl;

    double *pB = B.ptr<double>();
    m_AfrB(0,0) = pB[0]; m_AfrB(0,1) = pB[1]; m_AfrB(0,2) = pB[2]; m_AfrB(0,3) = pB[3];
    m_AfrB(1,0) = pB[4]; m_AfrB(1,1) = pB[5]; m_AfrB(1,2) = pB[6]; m_AfrB(1,3) = pB[7];
    m_AfrB(2,0) = pB[8]; m_AfrB(2,1) = pB[9]; m_AfrB(2,2) = pB[10]; m_AfrB(2,3) = pB[11];
    m_AfrB(3,0) =0     ; m_AfrB(3,1) = 0    ; m_AfrB(3,2) = 0     ; m_AfrB(3,3) = 1;
    //cout << "m_AfrB: " << m_AfrB << endl;
}

Matrix4d RigidCoordCalibration::calMatrixRotateJ5(double angleJ5, double l)
{
    // 先向反方向 z 移动 l => m1, 然后旋转 => m2, 再移动l回去 => m3
    Matrix4d m1, m3;
    m1.identity(); m3.identity();
    m1(2, 3) = -l;
    m3(2, 3) = l;

    Matrix4d m2 = MeTransform::rotateY(angleJ5);

    Matrix4d m = m1*m2*m3;
    return m;
}

Matrix4d RigidCoordCalibration::calMatrixRotateJ4(double angleJ4, double l)
{
    // 先向反方向 z 移动 l => m1, 然后旋转 => m2, 再移动l回去 => m3
    Matrix4d m1, m3;
    m1.identity(); m3.identity();
    m1(1, 3) = -l;
    m3(1, 3) = l;

    Matrix4d m2 = MeTransform::rotateX(-angleJ4);

    Matrix4d m = m1*m2*m3;
    return m;
}
